/**
 * @file SamplingPlanner.cpp
 * @brief 
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2021-02-18
 * 
 * @copyright Copyright (c) 2021  WHU-EIS
 * 
 */
#include "SamplingPlanner.h"
#include <pluginlib/class_list_macros.h>
#include <algorithm>
#include <nav_msgs/Path.h>
#include <visualization_msgs/MarkerArray.h>

// 将规划器注册为插件
PLUGINLIB_EXPORT_CLASS(global_planner::SamplingPlanner, 
                        nav_core::BaseGlobalPlanner);


namespace global_planner{

/**
 * @brief Construct a new Sampling Planner:: Sampling Planner object
 */
SamplingPlanner::SamplingPlanner(){

}

/**
 * @brief Construct a new Sampling Planner:: Sampling Planner object
 * @param  name             规划器名称
 * @param  costmap_ros      代价地图指针
 */
SamplingPlanner::SamplingPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
    initialize(name, costmap_ros);
}

/**
 * @brief 规划器初始化函数，从基类中重载
 * @param  name             规划器名称
 * @param  costmap_ros      代价地图指针
 */
void SamplingPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
    if(!initialized_){
        costmap_ros_ = costmap_ros;
        costmap_ = costmap_ros_->getCostmap();

        // 初始化规划器参数
        ros::NodeHandle private_nh("~/" + name);
        private_nh.param("step_size", step_size_, costmap_->getResolution());
        private_nh.param("min_dist_from_robot", min_dist_from_robot_, 0.10);
        // private_nh.param("path_points_path", path_points_path, std::string(""));
        world_model_ = new base_local_planner::CostmapModel(*costmap_);

        ros::NodeHandle nh("~");
        nh.param("path_points_path", path_points_path, std::string(""));


        plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);

        initialized_ = true;
    }
    else
        ROS_WARN("This planner has already been initialized... doning nothing");


}

/**
 * @brief 规划路径函数，从基类中重载
 * @param  start            起点
 * @param  goal             终点
 * @param  plan             规划得到的路径
 * @return true 
 * @return false 
 */
bool SamplingPlanner::makePlan( const geometry_msgs::PoseStamped& start,
                                const geometry_msgs::PoseStamped& goal,
                                std::vector<geometry_msgs::PoseStamped>& plan)
{

    boost::mutex::scoped_lock lock(mutex_);
    if(!initialized_){
        ROS_ERROR("This planner has not been initialized yet, but it's being used. Please call initialize() befor use.");
        return false;
    }
    plan.clear();
    frame_id_ = start.header.frame_id;

    // ROS_INFO("path_points_path: %s", path_points_path.c_str());

    plan.push_back(start);
    std::ifstream inputFile(path_points_path);
    while (!inputFile.eof()) {        
        // ROS_INFO("ok");
        double x, y;
        inputFile >> x >> y;

        if(x != 0 && y != 0){
            geometry_msgs::PoseStamped point;
            point.pose.orientation.w = 1;
            point.pose.position.x = x;
            point.pose.position.y = y;

            plan.push_back(point);
        }
    }
    ROS_INFO("plan length: %d", plan.size());
    inputFile.close();
    plan.push_back(goal);
    publishPlan(plan);
    return true;

    // plan.push_back(start);
    // std::ifstream inputFile(path_points_path);
    // while (!inputFile.eof()) {        
    //     // ROS_INFO("ok");
    //     double latitude, longitude;
    //     inputFile >> latitude >> longitude;

    //     // BLH转换到当地XY坐标系
    //     Eigen::Vector3d point_blh, point_xy;
    //     point_blh << (latitude / 180.0 * M_PI), (longitude / 180.0 * M_PI), 0;
    //     blh2locpos_.BLH2xy(point_blh, point_xy);

    //     geometry_msgs::PoseStamped point;
    //     point.pose.orientation.w = 1;
    //     point.pose.position.x = point_xy.x();
    //     point.pose.position.y = point_xy.y();

    //     plan.push_back(point);
    // }
    // ROS_INFO("plan length: %d", plan.size());
    // inputFile.close();
    // plan.push_back(goal);
    // publishPlan(plan);
    // return true;
}

/**
 * @brief 发布路径点话题
 * @param  path             规划得到的路径
 */
void SamplingPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path) {
    if (!initialized_) {
        ROS_ERROR(
                "This planner has not been initialized yet, but it is being used, please call initialize() before use");
        return;
    }

    //create a message for the plan
    nav_msgs::Path gui_path;
    gui_path.poses.resize(path.size());

    gui_path.header.frame_id = frame_id_;
    gui_path.header.stamp = ros::Time::now();

    // Extract the plan in world co-ordinates, we assume the path is all in the same frame
    for (unsigned int i = 0; i < path.size(); i++) {
        gui_path.poses[i] = path[i];
    }

    plan_pub_.publish(gui_path);
}

/**
 * @brief 从地图坐标系转换到世界坐标系
 * @param  mx               地图x坐标
 * @param  my               地图y坐标
 * @param  wx               世界x坐标的引用
 * @param  wy               世界y坐标的引用
 */
void SamplingPlanner::mapToWorld(double mx, double my, double& wx, double& wy) {
    wx = costmap_->getOriginX() + (mx+convert_offset_) * costmap_->getResolution();
    wy = costmap_->getOriginY() + (my+convert_offset_) * costmap_->getResolution();
}
/**
 * @brief 从世界坐标系转换到地图坐标系
 * @param  wx               世界x坐标
 * @param  wy               世界y坐标
 * @param  mx               地图x坐标的引用
 * @param  my               地图y坐标的引用
 * @return true 
 * @return false 
 */
bool SamplingPlanner::worldToMap(double wx, double wy, double& mx, double& my) {
    double origin_x = costmap_->getOriginX(), origin_y = costmap_->getOriginY();
    double resolution = costmap_->getResolution();

    if (wx < origin_x || wy < origin_y)
        return false;

    mx = (wx - origin_x) / resolution - convert_offset_;
    my = (wy - origin_y) / resolution - convert_offset_;

    if (mx < costmap_->getSizeInCellsX() && my < costmap_->getSizeInCellsY())
        return true;

    return false;
}


}